#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <signal.h>
#include <time.h>
#include <sicklms-1.0/SickLMS.hh>

#include "maestro.h"
#include "compass.h"
#include "gotemp.h"
#include "gps_boat.h"
#include "config.h"

#include <gps.h>
#include "pot_field.h"
#include "main.h"




boat myBoat;

int maestro;
int compass;
unsigned int commandDirection, commandStepper;
FILE * logFile;
FILE * statusFile;
FILE * gpsFile;
FILE * sickFile=fopen("/home/actuv/boats code/New Boats Code/Outputs/MinDistance","w+");

VECTOR origin;
double bias = 0;

bool isObs, isRotate;

extern VECTOR goal;
extern VECTOR obs;

using namespace std;
using namespace SickToolbox;

SickLMS *lmsp;

int init(){
	fprintf(stderr, "Initializing.\n");
//	signal(SIGINT, signal_handler);
	maestro = maestro_connect();
	fprintf(stderr, "M:%d\n", maestro);
	fprintf(stderr, "\t\tDevices connected, loading configuration.\n");

	loadConfig();
	fprintf(stderr, "\t\tConfiguration loaded.\n");
	myBoat.state = -1;
	myBoat.commandStarted = false;
	return 0;
}


int main(int argc, char* argv[]){
	char x[10];	
	init();
        usleep(1000); 
	/* Sick sensor initialization */
        string device_str;
                      
	setTarget(maestro, getDirection(), 0);
usleep(1000);
printf("%d\n", getPosition(maestro, getLimitswitch()));	
	while (1)
	{
/*gets(x);
printf("%d\n", getPosition(maestro, getLimitswitch()));
*/
                scanf("%d %d", &commandDirection, &commandStepper);
                printf("direction: %d, stepper: %d\n", commandDirection, commandStepper);	
		commandStepper = commandStepper * 3;
		setTarget(maestro, getDirection(), commandDirection * 1500);
//		setTarget(maestro, 0, );
usleep(2000);
                                    if (!getPosition(maestro, getLimitswitch()))
				    {
					printf("break!\n");					
					break;
				    }
usleep(2000);
setTarget(maestro, getDirection(), commandDirection * 1500);
		for(;commandStepper > 0; commandStepper--)		
		{

			setTarget(maestro, getStepper(), 2000);
			usleep(2000);
			setTarget(maestro, getStepper(), 0);
			usleep(2000);
		}
		printf("COMPLETED!\n");


	}

}

void printLog()
{
//	fprintf(stderr, "Printing Log.\n");

	fprintf(logFile, "<status>\n");
	time_t rawtime;
	struct tm * timeinfo;
	time ( &rawtime );
	timeinfo = localtime ( &rawtime );
	fprintf(logFile, "\t<compTime>%s</compTime>\n", asctime(timeinfo));
	
	time_t unixTime = myBoat.current.fix.time;
	fprintf(logFile, "\t<gpsTime>%s</gpsTime>\n", asctime(localtime(&unixTime)));

	switch(myBoat.state)
	{		
		case WAYPOINT:
			fprintf(logFile, "\t<state>Waypoint</state>\n");
			fprintf(logFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(logFile, "\t\t<commandPrecision>%lf</commandPrecision>\n", myBoat.commandArgs[2]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[3]); 
			break;
		case HEADING:
			fprintf(logFile, "\t<state>Heading</state>\n");
			fprintf(logFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[1]);
			break;
		case HALT:
			fprintf(logFile, "\t<state>Halt</state>\n");
			break;
		case ATSPEED:
			fprintf(logFile, "\t<state>At Speed</state>\n");
			fprintf(logFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[2]); 
			break;
		case ATHEADINGSPEED:
			fprintf(logFile, "\t<state>At Heading and Speed</state>\n");
			fprintf(logFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
			break;
		case ATHEADINGSPEEDMIXED:
			fprintf(logFile, "\t<state>At Heading and Speed, mixed</state>\n");
			fprintf(logFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
			break;
                case ATOBSAVOID:
        		fprintf(logFile, "\t<state>At Obstacle Avoidance, mixed</state>\n");
			fprintf(logFile, "\t\t<Goal><latitude>%lf</latitude>\n", goal.lat);
			fprintf(logFile, "\t\t<longitude>%lf</longitude></Goal>\n", goal.lon);
                        break;
		default:
			fprintf(logFile, "\t<state>Waiting</state>\n");
			break;
	}
	switch(myBoat.commandStarted)
	{
		case true: fprintf(logFile, "\t<started>1</started>\n"); break;
		case false: fprintf(logFile, "\t<started>0</started>\n"); break;
	}
	fprintf(logFile, "\t<lat>%lf</lat>\n", myBoat.current.fix.latitude);
	fprintf(logFile, "\t<lon>%lf</lon>\n", myBoat.current.fix.longitude);
	fprintf(logFile, "\t<compass>%lf</compass>\n", myBoat.retHeading);
	fprintf(logFile, "\t<temp>%lf</temp>\n", myBoat.tempProbe);

	fprintf(logFile, "</status>\n");

//	fprintf(stderr, "Wrote to log.\n");
}

void printStatus()
{
//	fprintf(stderr, "Printing status.\n");

	
	statusFile = fopen("Outputs/status.txt", "w");	
	fprintf(statusFile, "<status>\n");
	time_t rawtime;
	struct tm * timeinfo;
	time ( &rawtime );
	timeinfo = localtime ( &rawtime );
	fprintf(statusFile, "\t<compTime>%s</compTime>\n", asctime(timeinfo));

	time_t unixTime = myBoat.current.fix.time;
	fprintf(statusFile, "\t<gpsTime>%s</gpsTime>\n", asctime(localtime(&unixTime)));

	switch(myBoat.state)
	{		
		case WAYPOINT:
			fprintf(statusFile, "\t<state>Waypoint</state>\n");
			fprintf(statusFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(statusFile, "\t\t<commandPrecision>%lf</commandPrecision>\n", myBoat.commandArgs[2]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[3]); 
			break;
		case HEADING:
			fprintf(statusFile, "\t<state>Heading</state>\n");
			fprintf(statusFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[1]);
			break;
		case HALT:
			fprintf(statusFile, "\t<state>Halt</state>\n");
			break;
		case ATSPEED:
			fprintf(statusFile, "\t<state>At Speed</state>\n");
			fprintf(statusFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[2]); 
			break;
		case ATHEADINGSPEED:
			fprintf(statusFile, "\t<state>At Heading and Speed</state>\n");
			fprintf(statusFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
			break;
		case ATHEADINGSPEEDMIXED:
			fprintf(statusFile, "\t<state>At Heading and Speed, mixed</state>\n");
			fprintf(statusFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
                        break;
		case ATOBSAVOID:
			fprintf(statusFile, "\t<state>At Obstacle Avoidance</state>\n");
			fprintf(logFile, "\t\t<Goal><latitude>%lf</latitude>\n", goal.lat);
			fprintf(logFile, "\t\t<longitude>%lf</longitude></Goal>\n", goal.lon);
                        break;
		default:
			break;
	}
	switch(myBoat.commandStarted)
	{
		case true: fprintf(statusFile, "\t<started>1</started>\n"); break;
		case false: fprintf(statusFile, "\t<started>0</started>\n"); break;
	}
	fprintf(statusFile, "\t<lat>%lf</lat>\n", myBoat.current.fix.latitude);
	fprintf(statusFile, "\t<lon>%lf</lon>\n", myBoat.current.fix.longitude);
	fprintf(statusFile, "\t<compass>%lf</compass>\n", myBoat.retHeading);
	fprintf(statusFile, "\t<temp>%lf</temp>\n", myBoat.tempProbe);

	fprintf(statusFile, "</status>\n");
	fclose(statusFile);
//	fprintf(stderr, "Finished writing status.\n");
}

int getCommand()
{
//	fprintf(stderr, "Get Command.\n");
	FILE* fp = fopen("Commands/mission.txt", "r");
	if (fp == NULL)	{
		return 0;
	}	
	else {
		fclose(fp);
		myBoat.commandStarted = false;
		// Cleanup
		switch(myBoat.state){
			case WAYPOINT:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</waypoint>");
				break;
			case HEADING:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</heading>");
				break;
			case HALT:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</halt>");
				break;
			case ATSPEED:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atspeed>");
				break;
			case ATHEADINGSPEED:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atheadingspeed>");
				break;
			case ATHEADINGSPEEDMIXED:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atheadingspeedmixed>");
				break;
			case ATOBSAVOID:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atobsavoid>");
				break;
			default:
				break;
		}
		return 1;
	}
}	

int parseCommand()
{
	fprintf(stderr, "Parsing command.\n");
	rename( "Commands/mission.txt", "Commands/mission-parsing.txt");
	FILE* fp = fopen("Commands/mission-parsing.txt", "r");

	char str[MAX_LINE];
	while ((fgets(str, MAX_LINE, fp) != NULL))
	{
		//fprintf(stderr, "String: '%s' returned\n", str);
		char *pch;
		pch = strchr(str, '#');
		if (pch != NULL)
		{
			//fprintf(stderr, "Comment: %s", str);
		}
		else
		{
			int empty = strcmp(str, "\n");
			if (empty == 1)
			{
				char function[50];
				char args[50];

				sscanf(str, "%s \t %[^\n] \n", function, args);
				
				fprintf(stderr, "%s\n", args);
				if (!strcmp(function, "goToWaypoint"))
				{
					double lat = 1;
					double lon = 1;
					double precision = 1;// lon, precision;
					double speed = 1;
					sscanf(args, "[ %lf, %lf, %lf, %lf ]", &lat, &lon, &precision, &speed);
					//fprintf(stderr, "%f\n", lat);
					//fprintf(stderr, "Going to waypoint: %f,%f with %f precision, %f speed\n", lat, lon, precision, speed);
					
					myBoat.state = WAYPOINT;
					myBoat.commandArgs[0] = lat;
					myBoat.commandArgs[1] = lon;
					myBoat.commandArgs[2] = precision;
					myBoat.commandArgs[3] = speed;
					printLog();
					
				}
				else if (!strcmp(function, "goAtSpeed"))
				{
					double lat = 1;
					double lon = 1;
					double speed = 1;
					sscanf(args, "[ %lf, %lf, %lf ]", &lat, &lon, &speed);
					myBoat.state = ATSPEED;
					myBoat.commandArgs[0] = lat;
					myBoat.commandArgs[1] = lon;
					myBoat.commandArgs[2] = speed;
					printLog();
					
				}

				else if (!strcmp(function, "goHeadingSpeed"))
				{
					double heading = 0;
					double speed = 1;
					sscanf(args, "[ %lf, %lf]", &heading, &speed);
					myBoat.state = ATHEADINGSPEED;
					myBoat.commandArgs[0] = heading;
					myBoat.commandArgs[1] = speed;
					printLog();
					
				}

				else if (!strcmp(function, "goHeadingSpeedMixed"))
				{
					double heading = 0;
					double speed = 1;
					sscanf(args, "[ %lf, %lf]", &heading, &speed);
					myBoat.state = ATHEADINGSPEEDMIXED;
					myBoat.commandArgs[0] = heading;
					myBoat.commandArgs[1] = speed;
					printLog();
					
				}
				else if(!strcmp(function, "goToHeading"))
				{
					double desHeading;
					double speed = 1;
					sscanf(args, "[%lf, %lf]", &desHeading, &speed);
					//fprintf(stderr, "Going to desired heading of: %f\n", desHeading);

					myBoat.state = HEADING;
					myBoat.commandArgs[0] = desHeading;
					myBoat.commandArgs[1] = speed;
					printLog();
				}
				else if(!strcmp(function, "goObsAvoid"))
				{
					sscanf(args, "[%lf, %lf]", &goal.lat, &goal.lon);
                                        double distance = sqrt(goal.lat * goal.lat + goal.lon * goal.lon);
                                        double bearing = atan(goal.lon / goal.lat);

                                        if (goal.lat < 0)
                                            bearing += M_PI;

                                        bearing = bias * M_PI / 180 - bearing;
					fprintf(stderr, "bearing: %lf, distance: %lf\n", bearing, distance);
				        if (bearing < 0)
					    bearing = bearing + 2 * M_PI;
				        else if (bearing > 2 * M_PI)
					    bearing = bearing - 2 * M_PI;

                                        newPoint(origin.lat, origin.lon, bearing, distance, &goal);
					myBoat.state = ATOBSAVOID;
					printLog();
				}
				else if(!strcmp(function, "halt"))
				{
					myBoat.state = HALT;
					printLog();
				}
				else if (!strcmp(function, "setFrame"))
				{
					double latX = 1;
					double lonX = 1;
					sscanf(args, "[%lf, %lf, %lf, %lf]", &origin.lat, &origin.lon, &latX, &lonX);
					bias = calcHeading(origin.lat, origin.lon, latX, lonX);
    			                fprintf(logFile, "\t<state>Setframe</state>\n");
			                fprintf(logFile, "\t\t<origin>%lf, %lf</origin>\n", origin.lat, origin.lon);
	                                fprintf(stderr, "\t\t bias: %lf\n", bias); 
					fprintf(stderr, "\t\t New frame set.\n");			
                                }
				else
				{
					fprintf(stderr, "cannot parse %s\n", function);
				}

			}
			else
			{
				//fprintf(stderr, "Empty line\n");
			}
		}
	}

	fclose(fp);


	rename( "Commands/mission-parsing.txt", "Commands/mission-running.txt");
	return 0;
}

double calcDistance(double lat1, double lon1, double lat2, double lon2)
{
	// Implemented using the haversine formula
	double radius = 637100;	

	lat1 = lat1 * M_PI/180;
	lat2 = lat2 * M_PI/180;
	lon1 = lon1 * M_PI/180;
	lon2 = lon2 * M_PI/180;
	double dLat = (lat2-lat1);
	double dLon = (lon2-lon1);
		
	double a = sin(dLat/2) * sin(dLat/2) +
		 sin(dLon/2) * sin(dLon/2) * cos(lat1) * cos(lat2);
	double c = 2 * atan2(sqrt(a), sqrt(1-a));


	return (c * radius);
}

double calcHeading(double lat1, double lon1, double lat2, double lon2)
{
	lat1 = lat1 * M_PI/180;
	lat2 = lat2 * M_PI/180;
	lon1 = lon1 * M_PI/180;
	lon2 = lon2 * M_PI/180;
	double dLon = lon2-lon1;
	//double dLat = lat2-lat1;

	double y = sin(dLon) * cos(lat2);
	double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
	double desiredHeading = (atan2(y, x) * 180/M_PI);
	return fmod(desiredHeading + 360.0, 360);
}

void fetchSick() {
    unsigned int values[SickLMS::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS.hh
    unsigned int num_values = 0;                                   // Holds the number of measurements returned
    int tempMin = 0;
    double tempI = 45; 
    int Xtemp = 0;
    int Ytemp = 0;


        /*
         * Acquire a few scans from the Sick LMS
         */
        try {


            lmsp->GetSickScan(values,num_values);      


            tempMin = values[0];

            for (unsigned int i=1; i<num_values; i++)
            {
                if((int)values[i] + TOLERANCE< tempMin)
                {
                    tempMin = values[i];
                    tempI = 90.0 / num_values * i+45;
                }

                //cout << "\t value:"<< values[i] << endl;  

            }
            
            Xtemp = cos((tempI*M_PI)/180)*tempMin;
            Ytemp = sin((tempI*M_PI)/180)*tempMin;

            if (abs(Ytemp) > LENGTH)
            {
                obs.lat = 0;
                obs.lon = 0;
                isObs = false;
                isRotate = false;
            }
            else if (!isObs)
            {
                if (abs(Xtemp) < WIDTH)
                {
                    obs.lat = Xtemp/1000.0;
                    obs.lon = Ytemp/1000.0;
                    isObs = true;
                    isRotate = true;
                }
                else if (abs(Xtemp) < SPREAD)
                {
                    obs.lat = Xtemp/1000.0;
                    obs.lon = Ytemp/1000.0;
                    isObs = true;
                }
                else
                {
                    obs.lat = 0;
                    obs.lon = 0;
                }
            }
            else if(isRotate)
            {
                if (abs(Xtemp > SPREAD))
                    isRotate = false;
                else
                {
                    obs.lat = Xtemp/1000.0;
                    obs.lon = Ytemp/1000.0;
                }
            }
            else
            {
                if (abs(Xtemp) > SPREAD + 500)
                {
                    obs.lat = 0;
                    obs.lon = 0;
                    isObs = false;
                    isRotate = false;
                }
                else if (abs(Xtemp) < WIDTH)
                {
                    obs.lat = Xtemp/1000.0;
                    obs.lon = Ytemp/1000.0;
                    isRotate = true;
                }
                else
                {
                    obs.lat = Xtemp/1000.0;
                    obs.lon = Ytemp/1000.0;
                }
            }

            sickFile=fopen("/home/actuv/boats code/New Boats Code/Outputs/MinDistance","w+");

            fprintf(sickFile,"%lf,%lf",obs.lat,obs.lon);
            fclose(sickFile);



            if (isObs)
                cout << "\t X:"<< obs.lat <<"\t Y:"<< obs.lon <<"\t Heading:"<< tempI << endl;  
            //cout << "\t X:"<< Xtemp <<"\t Y:"<< Ytemp << endl;

        }
        /* Catch anything else and exit */ 
        catch(...) {
            cerr << "An error occurred!" << endl;
        }

}

void signal_handler(int signum)
{

	setTarget(maestro, getRudder(), 1500);
	setTarget(maestro, getThrottle(), 1500);
	switch(myBoat.state){
		case WAYPOINT:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</waypoint>");
			fclose(logFile);
			break;
		case HEADING:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</heading>");
			fclose(logFile);
			break;
		case HALT:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</halt>");
			fclose(logFile);			
			break;
		case ATSPEED:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atspeed>");
			fclose(logFile);			
			break;
		case ATHEADINGSPEED:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atheadingspeed>");
			fclose(logFile);			
			break;
		case ATHEADINGSPEEDMIXED:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atheadingspeedmixed>");
			fclose(logFile);			
			break;
		case ATOBSAVOID:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atobsavoid>");
			fclose(logFile);			
			break;
                default:
			break;
	}
        

        /*
        * Uninitialize the device
        */
        try {
            lmsp->Uninitialize();
        }

        catch(...) {
            cerr << "Uninitialize failed!" << endl;
        }
	
        exit(signum);	
}

void newPoint(double lat, double lon, double bearing, double distance, VECTOR *newP) {
    lat = lat * M_PI / 180;
    lon = lon * M_PI / 180; 
    double radius = 6371000;
    double quot = distance / radius;
    newP->lat = asin(sin(lat) * cos(quot) + cos(lat) * sin(quot) * cos(bearing)) * 180 / M_PI;
    newP->lon = lon + atan2(sin(bearing) * sin(quot) * cos(lat), cos(quot) - sin(lat) * sin(newP->lat));
    newP->lon = newP->lon * 180 / M_PI;
}
